/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * File Name          : freertos.c
  * Description        : Code for freertos applications
  ******************************************************************************
  * @attention
  *
  * Copyright (c) 2022 STMicroelectronics.
  * All rights reserved.
  *
  * This software is licensed under terms that can be found in the LICENSE file
  * in the root directory of this software component.
  * If no LICENSE file comes with this software, it is provided AS-IS.
  *
  ******************************************************************************
  */
/* USER CODE END Header */

/* Includes ------------------------------------------------------------------*/
#include "FreeRTOS.h"
#include "task.h"
#include "main.h"
#include "cmsis_os.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "stdio.h"
#include "usart.h"
#include "oled.h"
#include "system.h"
#include "action.h"
#include "bsp_PCA9685.h"
#include "bsp_PCA9685_0.h"
#include "tim.h"
#include "serial_servo.h"
/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */
int fputc(int ch,FILE *f)
{
    uint32_t temp = ch;
 
    HAL_UART_Transmit(&huart1,(uint8_t *)&temp,1,0xFFFF);        //huart1�Ǵ��ڵľ��
    HAL_Delay(2);
 
    return ch;
}
void HEXAPOD_Data_Processing(void);
void Printf_Data(void);

// �����ⲿ���� ���߶��������
extern SerialServoControllerTypeDef serial_servo_controller;

uint16_t testServoAngle = 100;
uint32_t testSetServoId = 4;
uint32_t testServoId = 4;
uint16_t testServoTime = 20;

// ���߶����ʼ��
void serial_servo_init(void);
void HEXAPOD_Data_Processing(void);
void Printf_Data(void);
/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
uint16_t PPM_Sample_Cnt = 0;//ͨ��
uint8_t PPM_Chn_Max = 8;//���ͨ����
uint32_t PPM_Time = 0;//��ȡͨ��ʱ��
uint16_t PPM_Okay = 0;//��һ�ν���״̬
uint16_t PPM_Databuf[8] = {0};//����ͨ��������
		float RS;
		float R0;
	
		float ppm;
/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN Variables */

/* USER CODE END Variables */
osThreadId defaultTaskHandle;
osThreadId myTask02_ledHandle;
osThreadId myTask03_printHandle;
osThreadId myTask04Handle;

/* Private function prototypes -----------------------------------------------*/
/* USER CODE BEGIN FunctionPrototypes */

/* USER CODE END FunctionPrototypes */

void StartDefaultTask(void const * argument);
void StartTask02(void const * argument);
void StartTask03(void const * argument);
void StartTask04(void const * argument);

void MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */

/* GetIdleTaskMemory prototype (linked to static allocation support) */
void vApplicationGetIdleTaskMemory( StaticTask_t **ppxIdleTaskTCBBuffer, StackType_t **ppxIdleTaskStackBuffer, uint32_t *pulIdleTaskStackSize );

/* USER CODE BEGIN GET_IDLE_TASK_MEMORY */
static StaticTask_t xIdleTaskTCBBuffer;
static StackType_t xIdleStack[configMINIMAL_STACK_SIZE];

void vApplicationGetIdleTaskMemory( StaticTask_t **ppxIdleTaskTCBBuffer, StackType_t **ppxIdleTaskStackBuffer, uint32_t *pulIdleTaskStackSize )
{
  *ppxIdleTaskTCBBuffer = &xIdleTaskTCBBuffer;
  *ppxIdleTaskStackBuffer = &xIdleStack[0];
  *pulIdleTaskStackSize = configMINIMAL_STACK_SIZE;
  /* place for user code */
}
/* USER CODE END GET_IDLE_TASK_MEMORY */

/**
  * @brief  FreeRTOS initialization
  * @param  None
  * @retval None
  */
void MX_FREERTOS_Init(void) {
  /* USER CODE BEGIN Init */

  /* USER CODE END Init */

  /* USER CODE BEGIN RTOS_MUTEX */
  /* add mutexes, ... */
  /* USER CODE END RTOS_MUTEX */

  /* USER CODE BEGIN RTOS_SEMAPHORES */
  /* add semaphores, ... */
  /* USER CODE END RTOS_SEMAPHORES */

  /* USER CODE BEGIN RTOS_TIMERS */
  /* start timers, add new ones, ... */
  /* USER CODE END RTOS_TIMERS */

  /* USER CODE BEGIN RTOS_QUEUES */
  /* add queues, ... */
  /* USER CODE END RTOS_QUEUES */

  /* Create the thread(s) */
  /* definition and creation of defaultTask */
  osThreadDef(defaultTask, StartDefaultTask, osPriorityNormal, 0, 128);
  defaultTaskHandle = osThreadCreate(osThread(defaultTask), NULL);

  /* definition and creation of myTask02_led */
  osThreadDef(myTask02_led, StartTask02, osPriorityLow, 0, 128);
  myTask02_ledHandle = osThreadCreate(osThread(myTask02_led), NULL);

  /* definition and creation of myTask03_print */
  osThreadDef(myTask03_print, StartTask03, osPriorityLow, 0, 128);
  myTask03_printHandle = osThreadCreate(osThread(myTask03_print), NULL);

  /* definition and creation of myTask04 */
  osThreadDef(myTask04, StartTask04, osPriorityLow, 0, 128);
  myTask04Handle = osThreadCreate(osThread(myTask04), NULL);

  /* USER CODE BEGIN RTOS_THREADS */
  /* add threads, ... */
  /* USER CODE END RTOS_THREADS */

}

/* USER CODE BEGIN Header_StartDefaultTask */
/**
  * @brief  Function implementing the defaultTask thread.
  * @param  argument: Not used
  * @retval None
  */
/* USER CODE END Header_StartDefaultTask */
void StartDefaultTask(void const * argument)
{
  /* USER CODE BEGIN StartDefaultTask */
	#ifdef busServo
	uint32_t broadcast_id = 0xFE; //����Ϊ�㲥ID�����ɸ���
//	int ret = 0;
	uint8_t servo_id = 99;
	/* ���ڶ�����Ƴ�ʼ�� */
	serial_servo_init();
	#endif
  /* Infinite loop */
  for(;;)
  {
		#ifdef enableHexapodDisableQuadrupled
		Gait_update(-Blue_x,-Blue_y,-Blue_z,RxBuff[0]);
		Gait_plan();
		#endif
		if(robotStartFlag == 1)
		{
//			Servo_Change();
			#ifdef busServo
			#else
			{
				for(Pca9685_n=0;Pca9685_n<18;Pca9685_n++)
				{
					//addr1:ģ���ַ,n��ͨ��,m������Ƕ�����
					pca_setpwm(addr1,Pca9685_n,Servo[Pca9685_n]);	//Servo[Pca9685_n]
				}
				osDelay(10);
			}
			#endif
		}
  }
  /* USER CODE END StartDefaultTask */
}

/* USER CODE BEGIN Header_StartTask02 */
/**
* @brief Function implementing the myTask02_led thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_StartTask02 */
void StartTask02(void const * argument)
{
  /* USER CODE BEGIN StartTask02 */
  /* Infinite loop */
  for(;;)
  {	
		//Read_DMP();
		HEXAPOD_Data_Processing();
		Lidar_Avoid();
		Voice_Control();
		//nec_control();
    osDelay(10);
  }
  /* USER CODE END StartTask02 */
}

/* USER CODE BEGIN Header_StartTask03 */
/**
* @brief Function implementing the myTask03_print thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_StartTask03 */
void StartTask03(void const * argument)
{
  /* USER CODE BEGIN StartTask03 */
  /* Infinite loop */
  for(;;)
  {
		#ifdef enableOLED
		OLED_ShowString(0,0,"Test VoiceCrtl",16);
		OLED_ShowString(0,2,"Input",16); 					OLED_ShowNum(120,2,(int)Voice_PA0,1,16);
															OLED_ShowNum(112,2,(int)Voice_PA1,1,16);
															OLED_ShowNum(104,2,(int)Voice_PA2,1,16);
															OLED_ShowNum(96,2,(int)Voice_PA3,1,16);
		OLED_ShowString(0,4,"Mode",16);
		if(PS2_flag)
		{
			OLED_ShowString(48,4,"P S 2",16);
		}
		else if(Lidar_Avoid_flag)
		{
			OLED_ShowString(48,4,"Lidar",16);
		}
		else if(NEC_flag)
		{
			OLED_ShowString(48,4,"N E C",16);
		}
		else if(Voice_flag)
		{
			OLED_ShowString(48,4,"Voice",16);
		}
		OLED_ShowNum(120,4,(int)RxBuff[0],1,16);
		
		RS = (3.3-VoltageValue[0])/(VoltageValue[0]*0.5);
		R0=6.64;
	
		ppm = pow(11.5428*R0/RS, 0.6549f);
		OLED_ShowString(0,6,"MQ135-PPM",16);				OLED_ShowNum(96,6,(int)(ppm*10),3,16);
		#endif
		#ifdef enableROS
		data_transition(); 
		USART1_SEND();
		#else
		Printf_Data();
		#endif

		osDelay(200);		
  }
  /* USER CODE END StartTask03 */
}

/* USER CODE BEGIN Header_StartTask04 */
/**
* @brief Function implementing the myTask04 thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_StartTask04 */
void StartTask04(void const * argument)
{
  /* USER CODE BEGIN StartTask04 */
  /* Infinite loop */
  for(;;)
  {
		HAL_ADC_Start_DMA(&hadc1,(uint32_t*)uhADCxConvertedValue,2);
		VoltageValue[0] = uhADCxConvertedValue[0]*3.3f/4095;
		VoltageValue[1] = uhADCxConvertedValue[1]*3.3f/4095;
		Voltage = VoltageValue[1]*Voltage_num;
		if((Voltage < 11.1)&&(Voltage > 6))
		{
			Battery_flag = 0;
			PBout(0)=!PBout(0);
			PAout(4)=!PAout(4);
			osDelay(50);
		}
		else
		{
			Battery_flag = 1;
			Start_flag = 1;
			PBout(0)=!PBout(0);
			PAout(4) = 0;
			osDelay(1000);
		}
  }
  /* USER CODE END StartTask04 */
}

/* Private application code --------------------------------------------------*/
/* USER CODE BEGIN Application */
/* USER CODE BEGIN 4 */

void HEXAPOD_Data_Processing(void)
{
	PS2_KEY=PS2_DataKey();	
	if(PS2_KEY==4&&PS2_flag==0) 	//The start button on the // handle is pressed	//�ֱ��ϵ�Start����������
		PS2_Start_flag = 1; 
	if((PS2_Start_flag == 1)&&(PS2_flag == 0))		//ң����ʹ��
	{
		robotStartFlag 		= 1;
		PS2_flag 					= 1;
		RC_flag 					= 0;
		Lidar_Avoid_flag 	= 0;
	}
	if(PS2_KEY == 12)
	{
		Voice_flag 				= 1;
		PS2_flag					= 0;
		PS2_Start_flag 		= 0;
		Lidar_Avoid_flag 	= 0;
	}
	if(PS2_KEY == 10)
	{
		Voice_flag 				= 0;
		PS2_flag					= 1;
		PS2_Start_flag 		= 1;
		Lidar_Avoid_flag 	= 0;
	}
	if(PS2_KEY == 11)
	{
		Voice_flag 				= 0;
		PS2_flag					= 0;
		PS2_Start_flag 		= 0;
		Lidar_Avoid_flag 	= 1;
	}
	if(PS2_KEY == 9)
	{
		Voice_flag 				= 0;
		PS2_flag					= 1;
		PS2_Start_flag 		= 1;
		Lidar_Avoid_flag 	= 0;
	}
	
	if(PS2_flag == 1)
	{
		PS2_LX=PS2_AnologData(PSS_LX);    
		PS2_LY=PS2_AnologData(PSS_LY);
		PS2_RX=PS2_AnologData(PSS_RX);
		PS2_RY=PS2_AnologData(PSS_RY);
		PS2_KEY=PS2_DataKey();	
		if(PS2_KEY == 13)
		{
			osDelay(200);
			if(PS2_KEY == 13)
			{
				RxBuff[0] = RxBuff[0] + 1;
			}
			if(RxBuff[0] > 5)
			{
				RxBuff[0] = 0;
			}
		}
		if(PS2_KEY == 14)
		{
			RxBuff[0] = 2;
		}
		if(PS2_KEY == 15)
		{
			RxBuff[0] = 3;
		}
		if(PS2_KEY == 16)
		{
			RxBuff[0] = 4;
		}
		if(PS2_KEY == 5)			//OFFSET_Z
		{
			for(int i = 0;i < 6;i++)
			{
				HOME_Z[i] = HOME_Z[i] + 0.8f;
			}
		}
		if(PS2_KEY == 7)
		{
			for(int i = 0;i < 6;i++)
			{
				HOME_Z[i] = HOME_Z[i] - 0.8f;
			}
		}
		if(PS2_KEY == 11)
		{
			Lidar_Avoid_flag = 1;
		}
		if(PS2_KEY == 9)
		{
			Lidar_Avoid_flag = 0;
		}
	}
	
	if(PS2_flag == 1)
	{
	}
}


void Printf_Data(void)
{
//	printf("PS2_LX:%d ",PS2_LX);
//	printf("PS2_LY:%d ",PS2_LY);
//	printf("PS2_RX:%d ",PS2_RX);
//	printf("PS2_RY:%d ",PS2_RY);
//	printf("PS2_KEY:%d \r\n",PS2_KEY);
//	printf("current X: %f",HOME_X[0]);
//	printf("current Y: %f",HOME_Y[0]);
//	printf("current Z: %f",HOME_Z[0]);
//	printf("current Z: %f",TIBIA_LENGTH);
//	printf("current Yaw: %f",Yaw);
//	printf("%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\n",(float)Servo[6],(float)Servo[15],(float)Servo[0],(float)Servo[9],(float)Servo[3],(float)Servo[12]);
//			for (uint8_t i = 0; i < PPM_Chn_Max; i++)
//        printf("chn%d:%d ", i+1, PPM_Databuf[i]);
}

/* USER CODE END 4 */
/* USER CODE END Application */

